#include "s2dMesh.h"

#include "readFile.h"
#include "mesh.h"
#include "initVars.h"
#include <vector>
#include <cstdio>
#include <iostream>
#include <algorithm>
#include <string>
#include <cmath>

using namespace std;

int s2dMesh::xNum;
int s2dMesh::yNum;
double s2dMesh::boundaryRefineB;
double s2dMesh::boundaryLayer;
vector<vector<mesh::point2d> > s2dMesh::curveList;

int s2dMesh::meshInit()
{
    cout<<"Generating initial mesh."<<endl;
    xMesh=zeros2D(xNum,yNum);
    yMesh=zeros2D(xNum,yNum);

    mesh::point2d p0,p1,p2,p3;

    for(int k=0;k<xNum;k++){
        for(int p=0;p<yNum;p++){
            p0=curveList[0][k];
            p1=curveList[1][yNum-p-1];
            p2=curveList[2][xNum-k-1];
            p3=curveList[3][p];
            xMesh[k][p]=p0.x;
            double temp=double(p)/(double(yNum)-1);
            if(boundaryRefineB==0){
                yMesh[k][p]=p2.y+(p0.y-p2.y)*temp;
                continue;
            }
            yMesh[k][p]=
                p2.y+(p0.y-p2.y)
                *tanh(boundaryRefineB*temp)/tanh(boundaryRefineB);
        }
    }
    
    cout<<"Done."<<endl<<endl;
    // writeMesh();
    return 0;
}

int s2dMesh::getS2dMeshPara(vector<fileData> meshPara)
{   
    for(int k=0;k<meshPara.size();k++){
        if(meshPara[k].type=="xNum"){
            xNum=stoi(meshPara[k].value);
            cout<<"\tX number:"<<xNum<<endl;
        }
        if(meshPara[k].type=="yNum"){
            yNum=stoi(meshPara[k].value);
            cout<<"\tY number:"<<yNum<<endl;
        }
        if(meshPara[k].type=="boundaryRefineB"){
            boundaryRefineB=atof(meshPara[k].value.c_str());
        }
        if(meshPara[k].type=="boundaryLayer"){
            boundaryLayer=atof(meshPara[k].value.c_str());
        }
    }
    return 0;
}

int s2dMesh::writeMesh()
{
    cout<<"Write s2dMesh."<<endl;
    string path="./mesh/";
    path.append(to_string(s2dMeshOpt::iter));
    path.append("/");
    cout<<"\tMake dir\t"<<path<<endl;
    basic::dshMkdir(path);
    string file;

    file.assign(path);
    file.append("mesh");
    ofstream meshOut(file);
    meshOut<<"type\ts2dMesh"<<endl;
    meshOut<<"xNum\t"<<s2dMesh::xNum<<"\nyNum\t"<<s2dMesh::yNum<<endl;
    meshOut.close();

    file.assign(path);
    file.append("points");
    ofstream fout(file);
    for(int k=0;k<xNum;k++){
        for(int p=0;p<yNum;p++){
            fout<<setiosflags(ios::scientific)<<setprecision(mesh::writePrecision)<<xMesh[k][p]<<"\t"<<yMesh[k][p]<<endl;
        }
    }
    fout.close();
    
    cout<<"Done."<<endl<<endl;
    return 0;
}

int s2dMeshOpt::writeResidual()
{
    cout<<"Write residual."<<endl;
    string path="./mesh/";
    string file;

    file.assign(path);
    file.append("residual");
    ofstream resOut(file);

    for(int k=1;k<residual.size();k++){
        resOut<<k<<"\t"<<residual[k]<<endl;
    }

    resOut.close();

    cout<<"Done."<<endl<<endl;
    return 0;
}

int s2dMesh::writeMeshTemp()
{
    cout<<"Write temp mesh"<<endl;
    ofstream fout("./mesh/xTemp");
    streambuf *oldcout;
    oldcout = cout.rdbuf(fout.rdbuf());

    for(int k=0;k<xNum;k++){
        for(int p=0;p<yNum;p++){
            cout<<xMesh[k][p];
            if(p!=yNum-1){
                cout<<' ';
            }
            else{
                cout<<endl;
            }
        }
    }
    cout.rdbuf(oldcout);
    
    ofstream fout2("./mesh/yTemp");
    oldcout = cout.rdbuf(fout2.rdbuf());
    for(int k=0;k<xNum;k++){
        for(int p=0;p<yNum;p++){
            cout<<yMesh[k][p];
            if(p!=yNum-1){
                cout<<' ';
            }
            else{
                cout<<endl;
            }
        }
    }

    cout.rdbuf(oldcout);
    cout<<"Done."<<endl<<endl;
    return 1;
}

int s2dMesh::getCurvePoint(vector<mesh::point2d> wallPts,int num,bool straight)
{
    cout<<"Generate points for curve No."<<curveList.size()<<"."<<endl;
    vector<mesh::point2d> newWallPts;
    if(straight){
        vector<double> x=basic::linspace(wallPts[0].x,wallPts[1].x,num);
        vector<double> y=basic::linspace(wallPts[0].y,wallPts[1].y,num);

        newWallPts=basic::xyVecToCurve(x,y);
    }
    else{
        vector<double> x=basic::linspace(wallPts[0].x,wallPts[wallPts.size()-1].x,num);
        newWallPts=basic::interpCurve2d(wallPts,x,true);
    }
    cout<<"\tThere are "<<newWallPts.size()<<" points on the curve."<<endl;
    curveList.push_back(newWallPts);
    cout<<"Done."<<endl<<endl;
    return 0;
}

int s2dMeshOpt::optMesh()
{
    optMeshInit();
    cout<<"Opting mesh."<<endl;
    int bk=1;
    while(bk){
        cout<<"\tIter:\t"<<iter<<"\tResidual=";

        calcPhiPsi();
        bk=calcPQ();

        int temp=1;
        while(temp){
            copyBdryXY();
            temp=s2dMeshGS();
            copyXY();
            // calcBdrDiff();
            // calcItnDiff();
            updateBdrPoints();
        }
        
        calcBdrDiff();
        calcItnDiff();
        calcJ();
        calcABC();
        calcBdrPhiPsi();
        
        // calcBdrPQ();
        iter++;
        // if(iter%5000==0)
        //     s2dMesh::writeMesh();
        // break;
    }
    cout<<"Done."<<endl<<endl;
    iter=-1;
    s2dMesh::writeMesh();
    s2dMeshOpt::writeResidual();
    return 0;
}

int s2dMeshOpt::optMeshInit()
{
    cout<<"Opt mesh init."<<endl;
    aCoef=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    bCoef=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    cCoef=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    xMesh=s2dMesh::xMesh;
    yMesh=s2dMesh::yMesh;
    xMeshNew=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    yMeshNew=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    pSource=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    qSource=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    phi=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    psi=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    xKsi=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    xEta=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    yKsi=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    yEta=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    xKsiKsi=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    xEtaEta=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    yKsiKsi=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    yEtaEta=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    j=zeros2D(s2dMesh::xNum,s2dMesh::yNum);
    // calcItnABC();
    calcBdrDiff();
    calcBdrPhiPsi0();
    calcItnDiff();
    calcABC();
    calcJ();
    // calcBdrPQ();
    cout<<"Done."<<endl<<endl;
    return 0;
}

int s2dMeshOpt::calcBdrPhiPsi0()
{
    for(int k=0;k<s2dMesh::xNum;k++){
        int p=0;
        phi[k][p]=-(xKsi[k][p]*xKsiKsi[k][p]+yKsi[k][p]*yKsiKsi[k][p])/
            (pow(xEta[k][p],2)+pow(yEta[k][p],2));
        psi[k][p]=-(xEta[k][p]*xEtaEta[k][p]+yEta[k][p]*yEtaEta[k][p])/
            (pow(xKsi[k][p],2)+pow(yKsi[k][p],2));
        // cout<<phi[k][p]<<"\t"<<psi[k][p]<<endl;
    }
    return 0;
}

int s2dMeshOpt::calcBdrPhiPsi()
{
    double theta,d;
    for(int k=0;k<s2dMesh::xNum;k++){
        int p=0;
        d=sqrt(
            pow(xMesh[k][p+1]-xMesh[k][p],2)+
            pow(yMesh[k][p+1]-yMesh[k][p],2)
        );
        theta=acos(
            (xKsi[k][p]*xEta[k][p]+yKsi[k][p]*yEta[k][p])
            /
            (
                sqrt(pow(xKsi[k][p],2)+pow(yKsi[k][p],2))
                *
                sqrt(pow(xEta[k][p],2)+pow(yEta[k][p],2))
            )
        );
        // cout<<theta<<endl;
        phi[k][p]=phi[k][p]+sigma*tanh(thetaExp-theta)*90;
        // phi[k][p]=phi[k][p];
        // psi[k][p]=psi[k][p]-sigma*tanh(dExpB-d);
        psi[k][p]=psi[k][p];
        p=s2dMesh::yNum-1;
        d=sqrt(
            pow(xMesh[k][p]-xMesh[k][p-1],2)+
            pow(yMesh[k][p]-yMesh[k][p-1],2)
        );
        theta=acos(-
            (xKsi[k][p]*xEta[k][p]+yKsi[k][p]*yEta[k][p])
            /
            (
                sqrt(pow(xKsi[k][p],2)+pow(yKsi[k][p],2))
                *
                sqrt(pow(xEta[k][p],2)+pow(yEta[k][p],2))
            )
        );
        // cout<<k<<":\t"<<theta<<endl;
        phi[k][p]=phi[k][p]+sigma*tanh(thetaExp-theta)*1000;
        psi[k][p]=psi[k][p]+sigma*tanh(s2dMesh::boundaryLayer*yMesh[k][s2dMesh::yNum-1]-d)*100000;
        // cout<<s2dMesh::boundaryLayer<<endl;
    }
    return 0;
}

int s2dMeshOpt::calcPhiPsi()
{
    for(int k=0;k<s2dMesh::xNum;k++){
        for(int p=1;p<s2dMesh::yNum-1;p++){
            double temp=double(p)/(double(s2dMesh::yNum)-1);
            phi[k][p]=phi[k][0]+
                (phi[k][s2dMesh::yNum-1]-phi[k][0])*temp;
            // temp=double(k)/(double(s2dMesh::xNum)-1);
            psi[k][p]=psi[k][0]+
                (psi[k][s2dMesh::yNum-1]-psi[k][0])*temp;
        }
    }
    return 0;
}

int s2dMeshOpt::calcJ()
{
    for(int k=0;k<s2dMesh::xNum;k++){
        for(int p=0;p<s2dMesh::yNum;p++){
            j[k][p]=1/(xKsi[k][p]*yEta[k][p]-xEta[k][p]*yKsi[k][p]);
        }
    }
    return 0;
}

int s2dMeshOpt::calcBdrPQ()
{
    for(int p=1;p<s2dMesh::yNum-1;p++){
        int k=0;
        pSource[k][p]=aCoef[k][p]*phi[k][p]/
            pow(j[k][p],2);
        qSource[k][p]=cCoef[k][p]*psi[k][p]/
            pow(j[k][p],2);
        // cout<<"Bdr source: "<<pSource[k][p]<<"\t"<<qSource[k][p]<<endl;
        k=s2dMesh::xNum-1;
        pSource[k][p]=aCoef[k][p]*phi[k][p]/
            pow(j[k][p],2);
        qSource[k][p]=cCoef[k][p]*psi[k][p]/
            pow(j[k][p],2);
        // cout<<"Bdr source: "<<pSource[k][p]<<"\t"<<qSource[k][p]<<endl;
    }
    for(int k=0;k<s2dMesh::xNum;k++){
        int p=0;
        pSource[k][p]=aCoef[k][p]*phi[k][p]/
            pow(j[k][p],2);
        qSource[k][p]=cCoef[k][p]*psi[k][p]/
            pow(j[k][p],2);
        // cout<<"Bdr source: "<<pSource[k][p]<<"\t"<<qSource[k][p]<<endl;
        p=s2dMesh::yNum-1;
        pSource[k][p]=aCoef[k][p]*phi[k][p]/
            pow(j[k][p],2);
        qSource[k][p]=cCoef[k][p]*psi[k][p]/
            pow(j[k][p],2);
        // cout<<"Bdr source: "<<pSource[k][p]<<"\t"<<qSource[k][p]<<endl;
    }
    return 0;
}

int s2dMeshOpt::calcBdrDiff()
{
    for(int k=1;k<s2dMesh::xNum-1;k++){
        int p=0;
        xKsi[k][p]=(xMesh[k+1][p]-xMesh[k-1][p])/2;
        xKsiKsi[k][p]=xMesh[k+1][p]-2*xMesh[k][p]+xMesh[k-1][p];
        yKsi[k][p]=(yMesh[k+1][p]-yMesh[k-1][p])/2;
        yKsiKsi[k][p]=yMesh[k+1][p]-2*yMesh[k][p]+yMesh[k-1][p];
        xEta[k][p]=xMesh[k][p+1]-xMesh[k][p];
        xEtaEta[k][p]=xMesh[k][p+2]-2*xMesh[k][p+1]+xMesh[k][p];
        yEta[k][p]=yMesh[k][p+1]-yMesh[k][p];
        yEtaEta[k][p]=yMesh[k][p+2]-2*yMesh[k][p+1]+yMesh[k][p];
        p=s2dMesh::yNum-1;
        xKsi[k][p]=(xMesh[k+1][p]-xMesh[k-1][p])/2;
        xKsiKsi[k][p]=xMesh[k+1][p]-2*xMesh[k][p]+xMesh[k-1][p];
        yKsi[k][p]=(yMesh[k+1][p]-yMesh[k-1][p])/2;
        yKsiKsi[k][p]=yMesh[k+1][p]-2*yMesh[k][p]+yMesh[k-1][p];
        xEta[k][p]=xMesh[k][p]-xMesh[k][p-1];
        yEta[k][p]=yMesh[k][p]-yMesh[k][p-1];
        xEtaEta[k][p]=xMesh[k][p-2]-2*xMesh[k][p-1]+xMesh[k][p];
        yEtaEta[k][p]=yMesh[k][p-2]-2*yMesh[k][p-1]+yMesh[k][p];
    }
    for(int p=1;p<s2dMesh::yNum;p++){
        int k=0;
        xKsi[k][p]=xMesh[k+1][p]-xMesh[k][p];
        xKsiKsi[k][p]=xMesh[k+2][p]-2*xMesh[k+1][p]+xMesh[k][p];
        yKsi[k][p]=yMesh[k+1][p]-yMesh[k][p];
        yKsiKsi[k][p]=yMesh[k+2][p]-2*yMesh[k+1][p]+yMesh[k][p];
        xEta[k][p]=(xMesh[k][p+1]-xMesh[k][p-1])/2;
        xEtaEta[k][p]=xMesh[k][p+1]-2*xMesh[k][p]+xMesh[k][p-1];
        yEta[k][p]=(yMesh[k][p+1]-yMesh[k][p-1])/2;
        yEtaEta[k][p]=yMesh[k][p+1]-2*yMesh[k][p]+yMesh[k][p-1];
        k=s2dMesh::xNum-1;
        xKsi[k][p]=xMesh[k][p]-xMesh[k-1][p];
        yKsi[k][p]=yMesh[k][p]-yMesh[k-1][p];
        xKsiKsi[k][p]=xMesh[k-2][p]-2*xMesh[k-1][p]+xMesh[k][p];
        yKsiKsi[k][p]=yMesh[k-2][p]-2*yMesh[k-1][p]+yMesh[k][p];
        xEta[k][p]=(xMesh[k][p+1]-xMesh[k][p-1])/2;
        xEtaEta[k][p]=xMesh[k][p+1]-2*xMesh[k][p]+xMesh[k][p-1];
        yEta[k][p]=(yMesh[k][p+1]-yMesh[k][p-1])/2;
        yEtaEta[k][p]=yMesh[k][p+1]-2*yMesh[k][p]+yMesh[k][p-1];
    }
    int k=0;
    int p=0;
    xKsi[k][p]=xMesh[k+1][p]-xMesh[k][p];
    xKsiKsi[k][p]=xMesh[k+2][p]-2*xMesh[k+1][p]+xMesh[k][p];
    yKsi[k][p]=yMesh[k+1][p]-yMesh[k][p];
    yKsiKsi[k][p]=yMesh[k+2][p]-2*yMesh[k+1][p]+yMesh[k][p];
    xEta[k][p]=xMesh[k][p+1]-xMesh[k][p];
    xEtaEta[k][p]=xMesh[k][p+2]-2*xMesh[k][p+1]+xMesh[k][p];
    yEta[k][p]=yMesh[k][p+1]-yMesh[k][p];
    yEtaEta[k][p]=yMesh[k][p+2]-2*yMesh[k][p+1]+yMesh[k][p];
    p=s2dMesh::yNum-1;
    xKsi[k][p]=xMesh[k+1][p]-xMesh[k][p];
    yKsi[k][p]=yMesh[k+1][p]-yMesh[k][p];
    xKsiKsi[k][p]=xMesh[k+2][p]-2*xMesh[k+1][p]+xMesh[k][p];
    yKsiKsi[k][p]=yMesh[k+2][p]-2*yMesh[k+1][p]+yMesh[k][p];
    xEta[k][p]=xMesh[k][p]-xMesh[k][p-1];
    yEta[k][p]=yMesh[k][p]-yMesh[k][p-1];
    xEtaEta[k][p]=xMesh[k][p-2]-2*xMesh[k][p-1]+xMesh[k][p];
    yEtaEta[k][p]=yMesh[k][p-2]-2*yMesh[k][p-1]+yMesh[k][p];
    k=s2dMesh::xNum-1;
    xKsi[k][p]=xMesh[k][p]-xMesh[k-1][p];
    yKsi[k][p]=yMesh[k][p]-yMesh[k-1][p];
    xKsiKsi[k][p]=xMesh[k-2][p]-2*xMesh[k-1][p]+xMesh[k][p];
    yKsiKsi[k][p]=yMesh[k-2][p]-2*yMesh[k-1][p]+yMesh[k][p];
    xEta[k][p]=xMesh[k][p]-xMesh[k][p-1];
    yEta[k][p]=yMesh[k][p]-yMesh[k][p-1];
    xEtaEta[k][p]=xMesh[k][p-2]-2*xMesh[k][p-1]+xMesh[k][p];
    yEtaEta[k][p]=yMesh[k][p-2]-2*yMesh[k][p-1]+yMesh[k][p];
    p=0;
    xKsi[k][p]=xMesh[k][p]-xMesh[k-1][p];
    yKsi[k][p]=yMesh[k][p]-yMesh[k-1][p];
    xKsiKsi[k][p]=xMesh[k-2][p]-2*xMesh[k-1][p]+xMesh[k][p];
    yKsiKsi[k][p]=yMesh[k-2][p]-2*yMesh[k-1][p]+yMesh[k][p];
    xEta[k][p]=xMesh[k][p+1]-xMesh[k][p];
    xEtaEta[k][p]=xMesh[k][p+2]-2*xMesh[k][p+1]+xMesh[k][p];
    yEta[k][p]=yMesh[k][p+1]-yMesh[k][p];
    yEtaEta[k][p]=yMesh[k][p+2]-2*yMesh[k][p+1]+yMesh[k][p];
    return 0;
}

int s2dMeshOpt::calcItnDiff()
{
    for(int k=1;k<s2dMesh::xNum-1;k++){
        for(int p=1;p<s2dMesh::yNum-1;p++){
            xKsi[k][p]=(xMesh[k+1][p]-xMesh[k-1][p])/2;
            xKsiKsi[k][p]=xMesh[k+1][p]-2*xMesh[k][p]+xMesh[k-1][p];
            yKsi[k][p]=(yMesh[k+1][p]-yMesh[k-1][p])/2;
            yKsiKsi[k][p]=yMesh[k+1][p]-2*yMesh[k][p]+yMesh[k-1][p];
            xEta[k][p]=(xMesh[k][p+1]-xMesh[k][p-1])/2;
            xEtaEta[k][p]=xMesh[k][p+1]-2*xMesh[k][p]+xMesh[k][p-1];
            yEta[k][p]=(yMesh[k][p+1]-yMesh[k][p-1])/2;
            yEtaEta[k][p]=yMesh[k][p+1]-2*yMesh[k][p]+yMesh[k][p-1];
        }
    }
    return 0;
}

int s2dMeshOpt::calcItnABC()
{
    for(int k=1;k<s2dMesh::xNum-1;k++){
        for(int p=1;p<s2dMesh::yNum-1;p++){
            aCoef[k][p]=
                pow((xMesh[k][p+1]-xMesh[k][p-1])/2,2)+
                pow((yMesh[k][p+1]-yMesh[k][p-1])/2,2);
            bCoef[k][p]=
                (xMesh[k][p+1]-xMesh[k][p-1])/2*(xMesh[k+1][p]-xMesh[k-1][p])/2+
                (yMesh[k][p+1]-yMesh[k][p-1])/2*(yMesh[k+1][p]-yMesh[k-1][p])/2;
            cCoef[k][p]=
                pow((xMesh[k+1][p]-xMesh[k-1][p])/2,2)+
                pow((yMesh[k+1][p]-yMesh[k-1][p])/2,2);
        }
    }
    return 0;
}

int s2dMeshOpt::calcABC()
{
    for(int k=0;k<s2dMesh::xNum;k++){
        for(int p=0;p<s2dMesh::yNum;p++){
            aCoef[k][p]=
                pow(xEta[k][p],2)+
                pow(yEta[k][p],2);
            bCoef[k][p]=
                xKsi[k][p]*xEta[k][p]+
                yKsi[k][p]*yEta[k][p];
            cCoef[k][p]=
                pow(xKsi[k][p],2)+
                pow(yKsi[k][p],2);
            // cout<<"ABC: "<<aCoef[k][p]<<"\t"<<bCoef[k][p]<<"\t"<<cCoef[k][p]<<endl;
        }
    }
    return 0;
}

int s2dMeshOpt::calcPQ()
{
    double tempP,tempQ;
    double maxRes=0;
    //orig
    for(int k=0;k<s2dMesh::xNum;k++){
        for(int p=0;p<s2dMesh::yNum;p++){
            tempP=pSource[k][p];
            tempQ=qSource[k][p];
            pSource[k][p]=aCoef[k][p]*phi[k][p]/ //cCoef[k][p];
                pow(j[k][p],2);
            qSource[k][p]=cCoef[k][p]*psi[k][p]/ //aCoef[k][p];
                pow(j[k][p],2);
            double tempRes=max(abs((pSource[k][p]-tempP)/pSource[k][p]),abs((qSource[k][p]-tempQ)/qSource[k][p]));
            if(tempRes>maxRes){
                maxRes=tempRes;
            }
            // cout<<pSource[k][p]<<"  "<<qSource[k][p]<<endl;
        }
    }
    //interp
    //boundary
    // for(int k=0;k<s2dMesh::xNum;k++){
    //     int p=0;
    //     pSource[k][p]=aCoef[k][p]*phi[k][p]/ //cCoef[k][p];
    //             pow(j[k][p],2);
    //     qSource[k][p]=cCoef[k][p]*psi[k][p]/ //aCoef[k][p];
    //         pow(j[k][p],2);
    //     p=s2dMesh::yNum-1;
    //     pSource[k][p]=aCoef[k][p]*phi[k][p]/ //cCoef[k][p];
    //             pow(j[k][p],2);
    //     qSource[k][p]=cCoef[k][p]*psi[k][p]/ //aCoef[k][p];
    //         pow(j[k][p],2);
    // }
    // //internal
    // for(int k=0;k<s2dMesh::xNum;k++){
    //     for(int p=1;p<s2dMesh::yNum-1;p++){
    //         double temp=double(p)/(double(s2dMesh::yNum)-1);
    //         tempP=pSource[k][p];
    //         tempQ=qSource[k][p];
    //         pSource[k][p]=pSource[k][0]+
    //             (pSource[k][s2dMesh::yNum-1]-pSource[k][0])*temp;
    //         qSource[k][p]=qSource[k][0]+
    //             (qSource[k][s2dMesh::yNum-1]-qSource[k][0])*temp;
    //         double tempRes=max(abs(pSource[k][p]-tempP),abs(qSource[k][p]-tempQ));
    //         if(tempRes>maxRes){
    //             maxRes=tempRes;
    //             // cout<<tempRes<<endl;
    //         }
            // cout<<pSource[k][p]<<"  "<<qSource[k][p]<<endl;
        // }
    // }
    if(iter==1){
        pqResInit=maxRes;
    }
    else{
        maxRes=maxRes/pqResInit;
    }
    residual.push_back(maxRes);
    cout<<maxRes<<endl;
    if(maxRes<resMinPQ)
        return 0;
    return 1;
}

int s2dMeshOpt::calcPQInit()
{
    double tempP,tempQ;
    double maxRes=0;
    //orig
    for(int k=0;k<s2dMesh::xNum;k++){
        for(int p=0;p<s2dMesh::yNum;p++){
            tempP=pSource[k][p];
            tempQ=qSource[k][p];
            pSource[k][p]=aCoef[k][p]*phi[k][p]/ //cCoef[k][p];
                pow(j[k][p],2);
            qSource[k][p]=cCoef[k][p]*psi[k][p]/ //aCoef[k][p];
                pow(j[k][p],2);
            double tempRes=max(abs((pSource[k][p]-tempP)/pSource[k][p]),abs((qSource[k][p]-tempQ)/qSource[k][p]));
            if(tempRes>maxRes){
                maxRes=tempRes;
                // cout<<tempRes<<endl;
            }
            // cout<<pSource[k][p]<<"  "<<qSource[k][p]<<endl;
        }
    }
    // pqResInit=maxRes;
    // cout<<maxRes<<endl;
    return 1;
}

int s2dMeshOpt::s2dMeshGS()
{
    int bk=0;
    for(int k=1;k<s2dMesh::xNum-1;k++){
        for(int p=1;p<s2dMesh::yNum-1;p++){
            xMeshNew[k][p]=
                aCoef[k][p]
                    *(xMesh[k+1][p]+xMeshNew[k-1][p])
                -bCoef[k][p]/2
                    *(xMesh[k+1][p+1]-xMesh[k-1][p+1])
                +bCoef[k][p]/2
                    *(xMesh[k+1][p-1]-xMesh[k-1][p-1])
                +cCoef[k][p]
                    *(xMesh[k][p+1]+xMeshNew[k][p-1])
                -pSource[k][p];
            xMeshNew[k][p]=xMeshNew[k][p]
                /(2*(aCoef[k][p]+cCoef[k][p]));
            // cout<<xMeshNew[k][p]-xMesh[k][p]<<endl;
            xMeshNew[k][p]=xMesh[k][p]*(1-sorFac)+xMeshNew[k][p]*sorFac;
            yMeshNew[k][p]=
                aCoef[k][p]
                    *(yMesh[k+1][p]+yMeshNew[k-1][p])
                -bCoef[k][p]/2
                    *(yMesh[k+1][p+1]-yMesh[k-1][p+1])
                +bCoef[k][p]/2
                    *(yMesh[k+1][p-1]-yMesh[k-1][p-1])
                +cCoef[k][p]
                    *(yMesh[k][p+1]+yMeshNew[k][p-1])
                -qSource[k][p];
            yMeshNew[k][p]=yMeshNew[k][p]
                /(2*(aCoef[k][p]+cCoef[k][p]));
            yMeshNew[k][p]=yMesh[k][p]*(1-sorFac)+yMeshNew[k][p]*sorFac;
            // cout<<"Source: "<<pSource[k][p]<<"\t"<<qSource[k][p]<<endl;
            if(
                abs(xMeshNew[k][p]-xMesh[k][p])>resMin
                ||
                abs(yMeshNew[k][p]-yMesh[k][p])>resMin
            ){
                bk=1;
            }
        }
    }
    
    return bk;
}

int s2dMeshOpt::updateBdrPoints()
{
    for(int p=1;p<s2dMesh::yNum-1;p++){
        int k=0;
        double temp=yMesh[k][p];
        yMesh[k][p]=updateFac*
            ((yMesh[k+1][p]-yMesh[k+2][p])/
            (xMesh[k+1][p]-xMesh[k+2][p])*
            (xMesh[k][p]-xMesh[k+2][p])+
            yMesh[k+2][p]-yMesh[k][p])+yMesh[k][p];
        // cout<<p<<": "<<yMeshNew[k][p]<<endl;
        // cout<<"BDR update: "<<temp-yMeshNew[k][p]<<endl;
        k=s2dMesh::xNum-1;
        yMesh[k][p]=updateFac*
            ((yMesh[k-1][p]-yMesh[k-2][p])/
            (xMesh[k-1][p]-xMesh[k-2][p])*
            (xMesh[k][p]-xMesh[k-2][p])+
            yMesh[k-2][p]-yMesh[k][p])+yMesh[k][p];
    }
    return 0;
}

int s2dMeshOpt::copyBdryXY()
{
    for(int k=0;k<s2dMesh::xNum;k++){
        xMeshNew[k][0]=xMesh[k][0];
        xMeshNew[k][s2dMesh::yNum-1]=xMesh[k][s2dMesh::yNum-1];
        yMeshNew[k][0]=yMesh[k][0];
        yMeshNew[k][s2dMesh::yNum-1]=yMesh[k][s2dMesh::yNum-1];
    }
    for(int p=0;p<s2dMesh::yNum;p++){
        xMeshNew[0][p]=xMesh[0][p];
        xMeshNew[s2dMesh::xNum-1][p]=xMesh[s2dMesh::xNum-1][p];
        yMeshNew[0][p]=yMesh[0][p];
        yMeshNew[s2dMesh::xNum-1][p]=yMesh[s2dMesh::xNum-1][p];
    }
    return 0;
}

int s2dMeshOpt::copyXY()
{
    for(int k=0;k<s2dMesh::xNum;k++){
        for(int p=0;p<s2dMesh::yNum;p++){
            xMesh[k][p]=xMeshNew[k][p];
            yMesh[k][p]=yMeshNew[k][p];
        }
    }
    return 0;
}

vector<mesh::point2d> basic::interpCurve2d(vector<mesh::point2d> curve,vector<double> x,bool interpByX)
{
    vector<mesh::point2d> newCurve;
    if(interpByX){
        for(int k=0;k<x.size();k++){
            mesh::point2d tempPoint=basic::interpPoint2d(curve,x[k],interpByX);
            newCurve.push_back(tempPoint);
        }
    }
    else{
        cout<<"This feature is under development."<<endl<<endl;
    }
    return newCurve;
}

mesh::point2d basic::interpPoint2d(vector<mesh::point2d> curve,double x,bool interpByX)
{
    mesh::point2d newPoint;
    if(interpByX){
        newPoint.x=x;
        for(int k=1;k<curve.size();k++){
            if((x<=curve[k].x&&x>=curve[k-1].x)||(x>=curve[k].x&&x<=curve[k-1].x)){
                double y;
                y=(curve[k].y-curve[k-1].y)/(curve[k].x-curve[k-1].x)*(x-curve[k-1].x)+curve[k-1].y;
                // cout<<y<<endl;
                newPoint.y=y;
                break;
            }
            else{
                continue;
            }
        }
    }
    else{
        cout<<"This feature is under development."<<endl<<endl;
    }
    return newPoint;
}

vector<double> basic::linspace(double min,double max,int num)
{
    double temp;
    vector<double> res;
    res.push_back(min);
    for(int k=1;k<num-1;k++){
        temp=(max-min)/(num-1)*k+min;
        res.push_back(temp);
    }
    res.push_back(max);
    return res;
}

vector<mesh::point2d> basic::xyVecToCurve(vector<double> x,vector<double> y)
{
    vector<mesh::point2d> newCurve;
    mesh::point2d tempPt;
    for(int k=0;k<x.size();k++){
        tempPt.x=x[k];
        tempPt.y=y[k];
        newCurve.push_back(tempPt);
    }
    return newCurve;
}

